-- 当前为机器人手动模式代码,实现以下功能:
-- 1.机器人上伺服、下伺服操作.
-- 2.在笛卡尔坐标系下,机器人在X,Y,Z,Roll方向上实现连续移动,寸动移动的操作.
-- 3.在关节坐标系下,机器人在J1,J2,J3,J4方向上实现连续移动,寸动移动的操作.
-- 4.机器人移动到对应的目标点位.

function ManualMotion()
	
	--描述: 机器人关节寸动操作
	--输入:	inc	寸动的距离
	function RbtIncJointMove(inc)

		J1_P = GBool[3]		--J1/X+
		J2_P = GBool[4]		--J2/Y+
		J3_P = GBool[5] 		--J3/Z+
		J4_P = GBool[6]		--J4/A+
		J1_N = GBool[7] 		--J1/X-
		J2_N = GBool[8]		--J2/Y-
		J3_N = GBool[9]		--J3/Z-
		J4_N = GBool[10]		--J4/A- 

		--根据PLC发的信号在当前点位数值上增加步距坐标数值
		if J1_P then

			-- 往一轴正方向寸动指定数值
			Move.Axis(1, inc)
			Move.WaitForEOM()
			
		elseif J1_N then

			-- 往一轴负方向寸动指定数值
			Move.Axis(1, -1*inc)
			Move.WaitForEOM()

		elseif J2_P then	

			-- 往二轴正方向寸动指定数值
			Move.Axis(2, inc)
			Move.WaitForEOM()

		elseif J2_N then

			-- 往二轴负方向寸动指定数值
			Move.Axis(2, -1*inc)
			Move.WaitForEOM()

		elseif J3_P then	

			-- 往三轴负方向寸动指定数值(与机器人Z轴方向统一)
			Move.Axis(3, -1*inc)
			Move.WaitForEOM()

		elseif J3_N then

			-- 往三轴正方向寸动指定数值(与机器人Z轴方向统一)
			Move.Axis(3, inc)
			Move.WaitForEOM()

		elseif J4_P then

			-- 往四轴正方向寸动指定数值
			Move.Axis(4, inc)
			Move.WaitForEOM()

		elseif J4_N then

			-- 往四轴负方向寸动指定数值
			Move.Axis(4, -1*inc)
			Move.WaitForEOM()

		end 	
		
	end

	--描述: 机器人笛卡尔寸动操作
	--输入:	inc	寸动的距离
	function RbtIncLineMove(inc)

		IncCurrentLoc = Location.New()
		IncCurrentLoc:Here(1)

		X_P = GBool[3]		--J1/X+
		Y_P = GBool[4]		--J2/Y+
		Z_P = GBool[5] 		--J3/Z+
		ROLL_P = GBool[6]	--J4/A+
		X_N = GBool[7] 		--J1/X-
		Y_N = GBool[8]		--J2/Y-
		Z_N = GBool[9]		--J4/A-
		ROLL_N = GBool[10]	--J4/A- 

		--根据PLC发的信号在当前点位数值上增加步距坐标数值
		if X_P then

			-- 往X正方向寸动指定数值
			Move.LOffset(inc,0,0,0,0,0)
			Move.WaitForEOM()

		elseif X_N then

			-- 往X负方向寸动指定数值
			Move.LOffset(-1*inc,0,0,0,0,0)
			Move.WaitForEOM()

		elseif Y_P then	

			-- 往Y正方向寸动指定数值
			Move.LOffset(0,inc,0,0,0,0)
			Move.WaitForEOM()

		elseif Y_N then

			-- 往Y负方向寸动指定数值
			Move.LOffset(0,-1*inc,0,0,0,0)
			Move.WaitForEOM()

		elseif Z_P then	

			-- 往Z正方向寸动指定数值
			Move.LOffset(0,0,inc,0,0,0)
			Move.WaitForEOM()

		elseif Z_N then

			-- 往Z负方向寸动指定数值
			Move.LOffset(0,0,-1*inc,0,0,0)
			Move.WaitForEOM()

		elseif ROLL_P then

			-- 往Roll正方向寸动指定数值
			Move.Axis(4, inc)
			Move.WaitForEOM()

		elseif ROLL_N then

			-- 往Roll负方向寸动指定数值
			Move.Axis(4, -1*inc)
			Move.WaitForEOM()

		end
		 
	end

	--描述：机器人连续运动或寸动模式
	function JogMode()

		X_P = GBool[3]		  --J1/X+
		Y_P = GBool[4]		  --J2/Y+
		Z_P = GBool[5] 		  --J3/Z+
		ROLL_P = GBool[6]	  --J4/A+
		X_N = GBool[7] 		  --J1/X-
		Y_N = GBool[8]		  --J2/Y-
		Z_N = GBool[9]		  --J4/A-
		ROLL_N = GBool[10]	  --J4/A- 

		--判定有没有接收到单轴移动信号
		if X_P or Y_P or Z_P or ROLL_P or X_N or Y_N or Z_N or ROLL_N then
			bMoveByHand = true
		else
			bMoveByHand = false
		end 
		
		--判定有没有接收到bMoveByHand上升沿信号(设置步距寸动标志位)
		if bMoveByHand == true and bStep == false then 
			bIncMove = true
		else
			bIncMove = false
		end
		bStep = bMoveByHand	
	  
		if bMoveByHand then

			--示教方式选择(0：连续示教、1：寸动示教)
			TeachSelsct = GDouble[21]

			if TeachSelsct == 0 then

				--根据plc发的信号 使用单轴移动命令移动

				--启动单轴连续移动Jog控制模式
				Robot.JogMode(1,0,1)			

				--根据从PLC读取的8个单轴标志位和坐标系标志位bCartesianOrJoint移动

				--设置坐标系	
				--坐标系选择（1关节、2直角）
				RbtFrame = GDouble[25]

				if RbtFrame == 2 then
					Robot.Frame(1,2)
				elseif RbtFrame == 1 then
					Robot.Frame(1,1)
				end 	

				-- 为实时性考虑，之后的出动连续变量直接引用
				-- GBool[3]		J1/X+
				-- GBool[4]		J2/Y+
				-- GBool[5] 	J3/Z+
				-- GBool[6]		J4/A+
				-- GBool[7] 	J1/X-
				-- GBool[8]		J2/Y-
				-- GBool[9]		J4/A-
				-- GBool[10]	J4/A- 
				-- GDouble[26]  速度设置

				--往X+或1轴+方向运动
				while GBool[3] do
					GBool[64] = true
					if RbtFrame == 2 then	
					  
						Move.Jog("X",ReadSetSpeed)  		-- 笛卡尔坐标, X正方向, ReadSetSpeed%速度
					  
					elseif RbtFrame == 1 then
					  
						Move.Jog("J1",ReadSetSpeed)		-- 关节坐标, 1轴正方向, ReadSetSpeed%速度
					  
					end 
					Thread.Sleep(0.001)			
				end 

				--往X-或1轴-方向运动
				while GBool[7] do
					GBool[64] = true
					if RbtFrame == 2 then
					  
						Move.Jog("X",-1*ReadSetSpeed) 	-- 笛卡尔坐标, X负方向, ReadSetSpeed%速度   
					  
					elseif RbtFrame == 1 then
					  
						Move.Jog("J1",-1*ReadSetSpeed)	-- 关节坐标, 1轴负方向, ReadSetSpeed%速度	
								
					end 
					Thread.Sleep(0.001)
				end  

				--往Y+或2轴+方向运动
				while GBool[4] do
					GBool[64] = true
					if RbtFrame == 2 then
					  
						Move.Jog("Y",ReadSetSpeed)     	-- 笛卡尔坐标, Y正方向, ReadSetSpeed%速度
					  
					elseif RbtFrame == 1 then
					  
						Move.Jog("J2",ReadSetSpeed)	   	-- 关节坐标, 2轴正方向, ReadSetSpeed%速度
							  
					end 
					Thread.Sleep(0.001)
				end 

				--往Y-或2轴-方向运动
				while GBool[8] do
					GBool[64] = true
					if RbtFrame == 2 then

						Move.Jog("Y",-1*ReadSetSpeed)   		-- 笛卡尔坐标, Y负方向, ReadSetSpeed%速度
					  
					elseif RbtFrame == 1 then
					  
						Move.Jog("J2",-1*ReadSetSpeed)		-- 关节坐标, 2轴负方向, ReadSetSpeed%速度
						
					end 
					Thread.Sleep(0.001)
				end  

				--往Z+或3轴-方向运动,设置同一个运动方向,Z轴向上
				while GBool[5] do
					GBool[64] = true
					if RbtFrame == 2 then
					  
						Move.Jog("Z",ReadSetSpeed)      		-- 笛卡尔坐标, Z正方向, ReadSetSpeed%速度 
					  
					elseif RbtFrame == 1 then

						Move.Jog("J3",-1*ReadSetSpeed)  		-- 关节坐标, 3轴负方向, ReadSetSpeed%速度
						
					end 
					Thread.Sleep(0.001)
				end 

				--往Z-或3轴+方向运动,设置同一个运动方向,Z轴向下
				while GBool[9] do
					GBool[64] = true
					if RbtFrame == 2 then
					  
						Move.Jog("Z",-1*ReadSetSpeed)   	-- 笛卡尔坐标, Z负方向, ReadSetSpeed%速度 
					  
					elseif RbtFrame == 1 then
					  
						Move.Jog("J3",ReadSetSpeed)     	-- 关节坐标, 3轴正方向, ReadSetSpeed%速度
						  
					end 
					Thread.Sleep(0.001)	
				end

				--往Roll+或4轴+方向运动,设置同一个运动方向,顺时针转动
				while GBool[6] do
					GBool[64] = true
					if RbtFrame == 2 then
					  
						Move.Jog("Roll",ReadSetSpeed)    	-- 笛卡尔坐标, Roll正方向, ReadSetSpeed%速度 

					elseif RbtFrame == 1 then
					  
						Move.Jog("J4",ReadSetSpeed)     	-- 关节坐标, 4轴正方向, ReadSetSpeed%速度
					  
					end 
					Thread.Sleep(0.001)
				end 

				--往Roll-或4轴-方向运动,设置同一个运动方向,逆时针转动
				while GBool[10] do
					GBool[64] = true
					if RbtFrame == 2 then
					  
						Move.Jog("Roll",-1*ReadSetSpeed)  		-- 笛卡尔坐标, Roll负方向, ReadSetSpeed%速度 
					  
					elseif RbtFrame == 1 then
					  
						Move.Jog("J4",-1*ReadSetSpeed)	  		-- 关节坐标, 4轴负方向, ReadSetSpeed%速度
					  
					end 
					Thread.Sleep(0.001)
				end  

				--关闭jogmode
				Move.StopJog()
				Thread.Sleep(1)
				Robot.Frame(1,2)

				Robot.JogMode(1,0,0)
				Thread.Sleep(0.2)
				
				GBool[64] = false

			elseif TeachSelsct == 1 and bIncMove == true then			

				--设置坐标系	
				--坐标系选择（1关节、2直角）
				RbtFrame = GDouble[25]	

				if RbtFrame == 1 then

					MoveJInc = GDouble[22]/1000		--寸动关节步长
					RbtIncJointMove(MoveJInc)     		--机器人步距关节寸动操作  
				  
				elseif RbtFrame == 2 then

					MoveLInc = GDouble[23]/1000		--寸动线性步长
					RbtIncLineMove(MoveLInc)      		--机器人步距线性寸动操作		 

				end 

				bIncMove = false

			end 

		end
		
	end

	--描述：手动模块动作
	function RbtManualMotion()
		
	-- 手动模式信号
	ManualMode = GBool[1]
	
	--设置手动速度
	ReadSetSpeed = GDouble[27]
  
	if ManualMode == true then
	
		Bool_power, Bool_home = Robot.State(1)

		if Bool_power == 1 then
			RbtPowerOn = true
		else
			RbtPowerOn = false
		end
    
		if Bool_home == 1 then
			RbtHome = true
		else
			RbtHome = false
		end
		
		-- 当机器人处于上伺服状态且机器人未处于回零状态,则进行回零操作
		if (RbtPowerOn == true) then
			if (RbtHome == false) then
				Robot.Home(1)
			end
		end
		
		-- 读取的上电开关
		ReadPowerON = GBool[2]
    
		if ReadPowerON == true and RbtPowerOn == false then

			-- 机器人上伺服
			Robot.PowerEnable(1,1)			
			  
			-- 机器人回零
			Robot.Home(1)

		elseif ReadPowerON == false and RbtPowerOn == true then

			-- 机器人下伺服
			Robot.PowerEnable(1,0)		

		end
    
		-- 如果机器人处于以下状态,则执行手动功能:
		-- 1.手动状态.
		-- 2.无报警信号.
		-- 3.机器人成功上伺服.
		if ManualMode == true and IO.Get(DO_ALARM) == 0 and RbtPowerOn == true then
		  
			 --单轴移动控制("笛卡尔坐标&关节坐标"的"寸动&连续"功能)
			 JogMode()		
			
			 --读取PLC运动到该点的上升沿信号
			 bMoveToLoc = GBool[11]
			
			if bMoveToLoc == true and bOldOpt == false then 
				bOpt = true
			else
				bOpt = false
			end
			bOldOpt = bMoveToLoc
					
			if bOpt then

				--直接运动模式（1直线、2关节）GDouble[24]
				--StrMoveMode = GDouble[24]
				
				--根据读取的选择点位序号GDouble[26]到相应点位
				Point_Num = GDouble[26]
			
				--触发读取标志
				GBool[32] = true 
				itime = 0 
				while (GBool[32] == true) do 
					--延时3秒
					if itime > 15 then 
						goto endMove
					end 
					Thread.Sleep(0.2)
					itime = itime + 1 
				end
			
				--判断读取成功信号
				if GBool[33] == true then
					
					--读取到的点位存放在GString(26)
					strLoc = GString[26]
					locArray = Split(strLoc, ",")
					if tonumber(locArray[1]) ~= 0 and tonumber(locArray[2]) ~= 0 and tonumber(locArray[3]) ~= 0  and tonumber(locArray[4]) ~= 0 then 
						moveLoc.X  = tonumber(locArray[1])/1000
						moveLoc.Y  = tonumber(locArray[2])/1000
						moveLoc.Z  = tonumber(locArray[3])/1000
						moveLoc.Yaw = 0
						moveLoc.Pitch = 180
						moveLoc.Roll  = tonumber(locArray[4])/1000
					else
						goto endMove
					end 
					
					moveLoc.Config = 2
					
					GBool[23] = true
					UpToLoc(moveLoc)
					GBool[23] = false
					
					--[[
					--移动到安全点
					if Point_Num == 1 then
						UpToLoc(moveLoc)

					--移动到抓取点
					elseif Point_Num == 2 then
						UpToLoc(moveLoc)

					--移动到放置点1
					elseif Point_Num == 3 then
						UpToLoc(moveLoc)

					--移动到放置点2
					elseif Point_Num == 4 then
						UpToLoc(moveLoc)

					--移动到放置点3
					elseif Point_Num == 5 then
						UpToLoc(moveLoc)

					end
					--]]
				
				else
					print("移动到目标点的点位读取失败")
				end 
			
				::endMove::
				GBool[33] = false
				bOpt = false
		  
			end
      
		end
    
	end
    
end
	
	
	
	print("ManualMotion Thread Start!!!")

	Robot.Attached(1)
	
	-- 切换正确坐标系
	Robot.Frame(1,2)
	
	-- 机器人是否上伺服标志位
	RbtPowerOn = false

	bMoveByHand = false
	bIncMove = false
	bStep = false

	bOpt = false
	bOldOpt = false

	moveLoc = Location.New(0,0,0,0,180,0)

	-- 手动模块线程启动入口
	try{
		function()
	   
			--设置手动速度
			Manualprof = Profile.New(20,0.000,20,20,0.050,0.050,-1)
			Manualprof:Set(1)

			while true do
				
				RbtManualMotion()
				Thread.Sleep(0.05)

			end
	  
		end,
		
		catch = function(errors)
			
			-- 打印报错模块名称
			print("ManualMotion.Error")

			-- 打印报错代码所在文件名称
			print("file   :", errors.file)
			-- 打印报错代码行数
			print("line   :", errors.line)
			-- 打印报错信息
			print("message:", errors.message)
			-- 打印报错代码
			print("code   :", errors.code)
			-- 打印报错机器人
			print("robot  :", errors.robot)
			-- 打印报错轴数
			print("axis   :", errors.axis)
			
			ErrorCode = errors.code
			
			if ErrorCode ~= nil then
				ErrorCode = tonumber(errors.code)
			else
				ErrorCode = -20003
			end
			
			-- 超出限位的报错:
			-- 正方向超出限位
			if (ErrorCode == -5031) then
				ErrorAxis =  tonumber(errors.axis)
				if (ErrorAxis == 1) then
					ErrorCode = -20008
				elseif (ErrorAxis == 2) then
					ErrorCode = -20010
				elseif (ErrorAxis == 3) then
					ErrorCode = -20012
				elseif (ErrorAxis == 4) then
					ErrorCode = -20014
				end
			-- 负方向超出限位
			elseif (ErrorCode == -5032) then
				ErrorAxis =  tonumber(errors.axis)
				if (ErrorAxis == 1) then
					ErrorCode = -20009
				elseif (ErrorAxis == 2) then
					ErrorCode = -20011
				elseif (ErrorAxis == 3) then
					ErrorCode = -20013
				elseif (ErrorAxis == 4) then
					ErrorCode = -20015
				end
			end
			
			
			GBool[23] = false
			IO.Set(DO_HOMEFINISH,0)
			
			GDouble[3] = ErrorCode
			IO.Set(DO_ALARM,1)
			GBool[64] = false
		end,
			finally = function(ok,errors)
			
			
	end
		}

end